
#include "SpoolJoint.h"

namespace p2
{

IMPLEMENT_RTT(p2, SpoolJoint);

SpoolJoint::~SpoolJoint()
{
}

SpoolJoint* SpoolJoint::create(Anchor const& a0, Anchor const& a1, real radius,
		real lT, bool swapOrder, void* userData)
{
	SpoolJoint* newSpoolJoint = _allocator.allocate(1);
	if (newSpoolJoint) {
		_allocator.construct(newSpoolJoint, SpoolJoint(a0, a1, radius, lT, swapOrder, userData));
	}
	return newSpoolJoint;
}

/*
 * computeTangent:
 * computes a point on the tangent of a circle centered at (0, 0) and radius r which passes through a point outside the circumference
 * working:
 *
 */
Vector2<real> SpoolJoint::computeTangentPoint(Vector2<real> x, real r, real order)
{
	real r2 = r*r;
    real xx2 = x[0] * x[0];
    real xy2 = x[1] * x[1];
    ASSERTLOG((xx2+xy2)>=r2, "SpoolJoint::computeTangentPoint");
	real rxx = (r2*x[0] + order*r*x[1]*::sqrtf(xx2-r2+xy2)) / (xx2+xy2);
	real rxy = order * ((x[0] < rxx) ? +::sqrtf(r2 - rxx*rxx) : -::sqrtf(r2 - rxx*rxx));
	return Vector2<real>(rxx, rxy);
}

void SpoolJoint::preStep(real64 dT)
{
	PinJoint::preStep(dT);



}

bool SpoolJoint::solvePositions(real64 dT)
{
	computeFutureAnchorPositions(dT);

	/*
	 * work out the length to correct and check if it needs to be corrected
	 */
    real dth = _duth;
    if ( dth <= EPOS ) {
        return true;
    }
    _uth /= _duth;

    real dxth = Math<real>::Clamp(dth, -MAX_DISTANCE_CORRECTION, +MAX_DISTANCE_CORRECTION);

	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	_invK = Entity::invK(e0, e1, _rAth, _rBth, _uth);
	/*
	 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
	 * 'An impulse P that eliminates the distance d in a time step of length h must change
	 * the relative velocity of the two points by d / h.'
	 */
	real mag = -dxth / (real)dT * _invK;
	Vector2<real> impulse = mag * _uth;
	/*
	 * apply impulses
	 */
	if ( e0 ) {
		_dv[0].incV(+e0->getInvMass() * impulse);
		_dv[0].incW(+e0->getInvInertia() * _rAth.perp().dot(impulse));
	}
	if ( e1 ) {
		_dv[1].incV(-e1->getInvMass() * impulse);
		_dv[1].incW(-e1->getInvInertia() * _rBth.perp().dot(impulse));
	}

	return false;
}

bool SpoolJoint::solveVelocities(real64 dT)
{
	UNUSED(dT);
    Entity const* e0 = _anchor0.getEntity();
	//Entity const* e1 = _anchor1.getEntity();

	real effectImpulse = 0;
	effectImpulse += _mA * +Math<real>::Clamp(_radS - _radF, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
	if ( e0 ) {
		_dv[0].incW(-e0->getInvInertia() * effectImpulse);
	}

//	if (_flags & (FLAGS_ENABLE_MOTOR | FLAGS_ENABLE_FRICTION)) {
//		/*
//		 * apply friction or motor impulse
//		 */
//		real dw = (e1 ? (e1->getW()+_dv[1].getW()) : 0.f) - (e0 ? (e0->getW()+_dv[0].getW()) : 0.f);
//		real effectImpulse = 0;
//		if (_flags & FLAGS_ENABLE_MOTOR) {
//			/*
//			 * implement motor with specified angular velocity
//			 */
//			effectImpulse += _mA * +Math<real>::Clamp(_motorW - dw, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
//			/*
//			 * implement motor with specified torque
//			 */
//			effectImpulse += _motorTorque * (real)dT;
//		}
//		if (_flags & FLAGS_ENABLE_FRICTION) {
//			/*
//			 * implement friction as scaled mass and negative velocity product
//			 * as we will loose a scaled proportion of angular impulse through friction
//			 */
//			effectImpulse += _mA * _muA * -Math<real>::Clamp(dw, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
//		}
//		if ( e0 ) {
//			_dv[0].incW(-e0->getInvInertia() * effectImpulse);
//		}
//		if ( e1 ) {
//			_dv[1].incW(+e1->getInvInertia() * effectImpulse);
//		}
//	}
return false;
}

void SpoolJoint::computeFutureAnchorPositions(real64 dT)
{
	/*
	 *
	 */
	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();
    if ( e0 ) {
    	_xAth = e0->getX() + (e0->getV()+_dv[0].getV()) * (real)dT;
    } else {
    	_xAth = _anchor0.getPoint();
    }
    if ( e1 ) {
    	_xBth = e1->getX() + (e1->getV()+_dv[1].getV()) * (real)dT;
    	Matrix2<real> rot(e1->getA() + (e1->getW()+_dv[1].getW()) * (real)dT);
        _rBth = _anchor1.getPoint() * rot;
    } else {
    	_xBth = _anchor1.getPoint();
        _rBth = Vector2<real>::ZERO;
    }
    Vector2<real> xPth(_xBth + _rBth - _xAth);
	_uth = xPth - _rAth;
	_duth = _uth.length();
	/*
	 * compute length of thread on spool in radians
	 */
	_radS = (_lT - _duth) / _radius;
	if (_radS < 0) {
    	Matrix2<real> rot(e0->getA() + (e0->getW()+_dv[0].getW()) * (real)dT);
        _rAth = _anchor0.getPoint() * rot;
	} else {
		_rAth = _anchor0.getPoint();
	}
	//_rAth = SpoolJoint::computeTangentPoint(xPth, _radius, +_order);
	/*
	 * compute angle of point that thread leaves spool
	 */
	_radF = Math<real>::ATan2(_rAth[1], _rAth[0]);
}

} /* namespace p2 */
